#include "TSDA.h"
#include "BSP.h"

static volatile uint8_t IsReceived = 0;
static volatile uint8_t *pData;

// CAN中断回调函数
void TSDA_CAN_Callback(CAN_RxHeaderTypeDef *pHeader, uint8_t *pBuf)
{
    if (pHeader->StdId == 0x01)
    {
        IsReceived = 1;
        pData = pBuf;
    }
}

void TSDA_Start(void)
{
    uint8_t cmd[8] = {0};

    cmd[0] = TSDA_INDEX;
    cmd[1] = SEND_CODE;
    cmd[2] = 0x00;
    cmd[3] = 0x00;
    cmd[4] = 0x01;
    cmd[5] = 0xff;
    cmd[6] = 0x00;
    cmd[7] = 0x00;
    CAN_Transmit(&TSDA_CAN, TSDA_ID, cmd);
}

void TSDA_Close(void)
{
    uint8_t cmd[8] = {0};

    cmd[0] = TSDA_INDEX;
    cmd[1] = SEND_CODE;
    cmd[2] = 0x00;
    cmd[3] = 0x00;
    cmd[4] = 0x00;
    cmd[5] = 0xff;
    cmd[6] = 0x00;
    cmd[7] = 0x00;
    CAN_Transmit(&TSDA_CAN, TSDA_ID, cmd);
}

void TSDA_Stop(void)
{
    uint8_t cmd[8] = {0};

    cmd[0] = TSDA_INDEX;
    cmd[1] = SEND_CODE;
    cmd[2] = 0x4d;
    cmd[3] = 0x00;
    cmd[4] = 0x00;
    cmd[5] = 0xff;
    cmd[6] = 0x00;
    cmd[7] = 0x00;
    CAN_Transmit(&TSDA_CAN, TSDA_ID, cmd);
}

void TSDA_SetZero(void)
{
    uint8_t cmd[8] = {0};

    cmd[0] = TSDA_INDEX;
    cmd[1] = SEND_CODE;
    cmd[2] = 0x4c;
    cmd[3] = 0x00;
    cmd[4] = 0x00;
    cmd[5] = 0xff;
    cmd[6] = 0x00;
    cmd[7] = 0x00;
    CAN_Transmit(&TSDA_CAN, TSDA_ID, cmd);
}

void TSDA_SetMotorSpeed(float speed)
{
    int16_t v = (int16_t)(speed / 8192.0f * 3000.0f);

    uint8_t cmd[8] = {0};

    cmd[0] = TSDA_INDEX;
    cmd[1] = SEND_CODE;
    cmd[2] = 0x0a;
    cmd[3] = 1; // 0到3000RPM的加速时间是100ms
    cmd[4] = 1;
    cmd[5] = 0x06;
    cmd[6] = v >> 8;
    cmd[7] = v;
    CAN_Transmit(&TSDA_CAN, TSDA_ID, cmd);
}

// void TSDA_SetMaxSpeedInPostionMode(uint16_t MaxSpeed)
// {
//     uint8_t cmd[8] = {0};

//     cmd[0] = TSDA_INDEX;
//     cmd[1] = SEND_CODE;
//     cmd[2] = 0x1D;
//     cmd[3] = MaxSpeed >> 8;
//     cmd[4] = MaxSpeed;
//     cmd[5] = 0x00;
//     cmd[6] = 0x00;
//     cmd[7] = 0x01;
//     CAN_Transmit(&TSDA_CAN, TSDA_ID, cmd);
// }

void TSDA_SetMotorAngle(float angle)
{
    uint32_t Angle = (uint32_t)(angle / 360.0f * TSDA_PULSE);

    uint8_t cmd[8] = {0};

    cmd[0] = TSDA_INDEX;
    cmd[1] = SEND_CODE;
    cmd[2] = 0x50;
    cmd[3] = Angle >> 24;
    cmd[4] = Angle >> 16;
    cmd[5] = 0x05;
    cmd[6] = Angle >> 8;
    cmd[7] = Angle;
    CAN_Transmit(&TSDA_CAN, TSDA_ID, cmd);
}

void TSDA_SetPositonMode(uint8_t mode)
{
    uint8_t cmd[8] = {0};

    cmd[0] = TSDA_INDEX;
    cmd[1] = SEND_CODE;
    cmd[2] = 0x51;
    cmd[3] = 0x00;
    cmd[4] = mode;
    cmd[5] = 0x01;
    cmd[6] = 0x00;
    cmd[7] = 0x00;
    CAN_Transmit(&TSDA_CAN, TSDA_ID, cmd);
}

uint8_t TSDA_IsFinished(void)
{
    uint8_t cmd[8] = {0};

    cmd[0] = TSDA_INDEX;
    cmd[1] = READ_CODE;
    cmd[2] = 0x55;
    cmd[3] = 0x00;
    cmd[4] = 0x00;
    cmd[5] = 0xff;
    cmd[6] = 0x00;
    cmd[7] = 0x00;
    CAN_Transmit(&TSDA_CAN, TSDA_ID, cmd);

    while (!IsReceived)
        continue;
    IsReceived = 0;

    if (pData[4] == 1)
    {
        return 0;
    }

    return 1;
}

int16_t TSDA_ReadSpeed(void)
{
    uint8_t cmd[8] = {0};

    cmd[0] = TSDA_INDEX;
    cmd[1] = READ_CODE;
    cmd[2] = 0xe4;
    cmd[3] = 0x00;
    cmd[4] = 0x00;
    cmd[5] = 0xff;
    cmd[6] = 0x00;
    cmd[7] = 0x00;
    CAN_Transmit(&TSDA_CAN, TSDA_ID, cmd);

    while (!IsReceived)
        continue;
    IsReceived = 0;

    return (int16_t)(pData[3] << 8 | pData[4]);
}

int32_t TSDA_ReadAngle(void)
{
    uint8_t cmd[8] = {0};

    cmd[0] = TSDA_INDEX;
    cmd[1] = READ_CODE;
    cmd[2] = 0xe8;
    cmd[3] = 0x00;
    cmd[4] = 0x00;
    cmd[5] = 0xe9;
    cmd[6] = 0x00;
    cmd[7] = 0x00;
    CAN_Transmit(&TSDA_CAN, TSDA_ID, cmd);

    while (!IsReceived)
        continue;
    IsReceived = 0;

    return (int32_t)(pData[3] << 24 | pData[4] << 16 | pData[6] << 8 | pData[7]);
}
